package WayPoints;

import java.util.ArrayList;

import Movement.NERoom;

import lejos.nxt.LCD;
import lejos.robotics.navigation.MoveController;
import lejos.robotics.navigation.NavPathController;
import lejos.robotics.navigation.WayPoint;


public class NERecorder {
	//stock tous les chemins du point centrale aux diff�rentes pieces
	protected ArrayList<NERoom> listOfAllRooms;
	
	//stock le chemin actuel en train d'�tre enregistr�
	protected NERoom currentDestination;
	
	//id de la pi�ce que l'on enregistre
	protected int idActualRoom = -1;
	
	public NERecorder()
	{
		currentDestination = new NERoom();
		listOfAllRooms = new ArrayList<NERoom>();
		
	}
	
	// Constructeur pour faire les tests
	public NERecorder(ArrayList<WayPoint> Path1,ArrayList<WayPoint> Path2)
	{
		NERoom actualRoom = NERoom.newWithPath(Path1);
		this.listOfAllRooms = new ArrayList<NERoom>();
		listOfAllRooms.add(actualRoom);
		listOfAllRooms.add(NERoom.newWithPath(Path2));
		idActualRoom = 0; // On pr�tend �tre dans la salle du Path1 donc en indice 0 de l'ArrayList
		
	}
	
	//permet d'ajouter le chemin actuel dans la collection de tous les chemins
	public void recordPath()
	{
		listOfAllRooms.add(currentDestination);
		idActualRoom = listOfAllRooms.size()-1;
		currentDestination.setName(listOfAllRooms.size()-1);
		currentDestination = new NERoom();
		
	}
	
	public NERoom getDestination(int id)
	{
		return listOfAllRooms.get(id);
		
	}
	
	//ajoute un point dans le chemin actuel
	public void addWayPoint(WayPoint point)
	{
		currentDestination.addWayPoint(point);
		
	}
	
	//ajoute un point dans le chemin actuel
	public ArrayList<WayPoint> addWayPoints(ArrayList<WayPoint> listPath,ArrayList<WayPoint> bestPath,int indiceStartCopy)
	{
		//for(int i = indiceStartCopy + 1; i < listPath.size();i++)
		for(int i = indiceStartCopy; i < listPath.size();i++)
			{
				bestPath.add(listPath.get(i));
			}
		return bestPath;
	}
	
	
	// Calcul de l'intersection entre deux droites
	// Ordre des arguments : points A et B de la droite 1, puis A et B de la droite 2
	public ArrayList<Double> computeIntersection(WayPoint pA1, WayPoint pB1, WayPoint pA2, WayPoint pB2)
	{
		ArrayList<Double> result = new ArrayList<Double>();
		
		// Calcul de la premi�re droite a1 * x + b1
		if (pB1.getX() != pA1.getX() && (pB2.getX() != pA2.getX())) // si les droites 1 et 2 ne sont pas verticales
		{
			double a1 = (pB1.getY() - pA1.getY()) / (pB1.getX() - pA1.getX());
			double b1 = pB1.getY() - a1 * pB1.getX(); 
			
			// Calcul de la deuxi�me droite a2 * x + b2
			double a2 = (pB2.getY() - pA2.getY()) / (pB2.getX() - pA2.getX());
			double b2 = pB2.getY() - a2 * pB2.getX();
						
			// Calcul de l'intersection si les droites ne sont pas parrall�les
			if (a1 != a2)
			{
				double X = (b2 - b1) / (a1 - a2);
				double Y = (a1 * b2 - b1 * a2) / (a1 - a2);
				
				// On v�rifie que le point d'intersection est dans le segment [pA1 pB1]
				if (!((X <= pA1.getX() && X <= pB1.getX()) ||
					  (X >= pA1.getX() && X >= pB1.getX()) ||
					  (Y <= pA1.getY() && Y <= pB1.getY()) ||
					  (Y >= pA1.getY() && Y >= pB1.getY())))
				{
					// On v�rifie que le point d'intersection est dans le segment [pA2 pB2]
					if (!((X <= pA2.getX() && X <= pB2.getX()) ||
							  (X >= pA2.getX() && X >= pB2.getX()) ||
							  (Y <= pA2.getY() && Y <= pB2.getY()) ||
							  (Y >= pA2.getY() && Y >= pB2.getY())))
					{
						result.add(X);
						result.add(Y);
					}
				}
			}
			//on teste si les droites sont confondues
			else if (b1 == b2)
			{
				// On teste si le segment [pA1 pB1] est compris dans le segment [pA2 pB2]
				if (!((pA1.getX() < pA2.getX() && pA1.getX() < pB2.getX()) ||
					  (pA1.getX() > pA2.getX() && pA1.getX() > pB2.getX()) ||
				      (pA1.getY() < pA2.getY() && pA1.getY() < pB2.getY()) ||
				      (pA1.getY() > pA2.getY() && pA1.getY() > pB2.getY()))
				      &&
				    !((pB1.getX() < pA2.getX() && pB1.getX() < pB2.getX()) ||
					  (pB1.getX() > pA2.getX() && pB1.getX() > pB2.getX()) ||
					  (pB1.getY() < pA2.getY() && pB1.getY() < pB2.getY()) ||
					  (pB1.getY() > pA2.getY() && pB1.getY() > pB2.getY())))
				{
					result.add(pB2.getX());
					result.add(pB2.getY());
				}
			}
		}
	else if (pB1.getX() == pA1.getX() && (pB2.getX() != pA2.getX())) // Si droite 1 verticale et droite 2 pas verticale
	{
		// Calcul de la deuxi�me droite a2 * x + b2
		double a2 = (pB2.getY() - pA2.getY()) / (pB2.getX() - pA2.getX());
		double b2 = pB2.getY() - a2 * pB2.getX();
		
		double X = pB1.getX();
		double Y = a2 * X + b2;
		
		if((Y < pA1.getY() && Y > pB1.getY()) || (Y > pA1.getY() && Y < pB1.getY()))
		{
			result.add(X);
			result.add(Y);
		}
	}
	else if (pB1.getX() != pA1.getX() && (pB2.getX() == pA2.getX())) // Si droite 1 pas verticale et droite 2 verticale
	{
		double a1 = (pB1.getY() - pA1.getY()) / (pB1.getX() - pA1.getX());
		double b1 = pB1.getY() - a1 * pB1.getX(); 
		
		double X = pB2.getX();
		double Y = a1 * X + b1;
		
		if((Y < pA2.getY() && Y > pB2.getY()) || (Y > pA2.getY() && Y < pB2.getY()))
		{
			result.add(X);
			result.add(Y);
		}
	}
	// Si les droites 1 et 2 sont verticales
	else if (pB1.getX() == pA1.getX() && (pB2.getX() == pA2.getX())) 
	{
		// Si les droites sont confondues
		if (pB1.getX() == pB2.getX())
		{
			double X = pA1.getX();
			//TODO
			if ((pB1.getY() <= pA2.getY() && pB1.getY() >= pB2.getY()) || (pB1.getY() >= pA2.getY() && pB1.getY() <= pB2.getY()))
			{
				double Y = pB2.getY();
				result.add(X);
				result.add(Y);
			}
			else if ((pA1.getY() <= pA2.getY() && pA1.getY() >= pB2.getY()) || (pA1.getY() >= pA2.getY() && pA1.getY() <= pB2.getY()))
			{
				double Y = pA1.getY();
				result.add(X);
				result.add(Y);
			}
			else if ((pB2.getY() <= pA1.getY() && pB2.getY() >= pB1.getY()) || (pB2.getY() >= pA1.getY() && pB2.getY() <= pB1.getY()))
			{
				double Y = pB2.getY();
				result.add(X);
				result.add(Y);
			}
			else if ((pA2.getY() <= pA1.getY() && pA2.getY() >= pB1.getY()) || (pA2.getY() >= pA1.getY() && pA2.getY() <= pB1.getY()))
			{
				double Y = pA2.getY();
				result.add(X);
				result.add(Y);
			}
		}
	}
		return result;
	}
	
	
	//TODO
	
	//retourne le meilleur chemin possible en prenant pour param�tre l'identifiant de la salle o� on veut aller
	public ArrayList<WayPoint> returnBestPath(int idPathDestination)
	{
		//stockera les points du meilleur chemin possible
		ArrayList<WayPoint> bestPath = new ArrayList<WayPoint>();
				
		//le chemin vers la destination voulue
		ArrayList<WayPoint> destinationPath = new ArrayList<WayPoint>(listOfAllRooms.get(idPathDestination).getPath());
				
		//le chemin de l'endroit actuel au point central
		ArrayList<WayPoint> actualPath = new ArrayList<WayPoint>(listOfAllRooms.get(idActualRoom).getPath());
				
		//On parcourt le chemin d'origine en sens inverse
		for(int i = actualPath.size()-1;i >= 0;i--)
		{
			
			if (i != 0)
			{
				//On stock le point actuel du chemin 1
				WayPoint pointPath1 = actualPath.get(i);
				
				//On recup�re le point pr�c�dent du chemin 1
				WayPoint nextPointPath1 = actualPath.get(i-1);
				
				// On ajoute le point actuel au bestPath
				bestPath.add(pointPath1);
				
				
				for(int j = destinationPath.size()-1;j >= 0;j--)
				{
					if (j != 0)
					{
						//On stock le point actuel du chemin 2
						//WayPoint pointPath2 = actualPath.get(j);
						WayPoint pointPath2 = destinationPath.get(j);
						
						//On recup�re le point pr�c�dent du chemin 2
						//WayPoint nextPointPath2 = actualPath.get(j-1);
						WayPoint nextPointPath2 = destinationPath.get(j-1);
						
						//On r�cup�re le point d'intersection
						ArrayList<Double> intersection = new ArrayList<Double>();
						intersection = computeIntersection(pointPath1, nextPointPath1, nextPointPath2, pointPath2);
						
						// On v�rifie si une intersection a �t� trouv�e, sinon on ne fait rien et on va tester la prochaine droite du chemin 2 avec le for
						if (intersection.size() != 0)
						{
							// On ajoute l'intersection au best path
							WayPoint wp = new WayPoint(intersection.get(0), intersection.get(1));
							// Si l'intersection est diff�rente du dernier WayPoint ins�r� dans le bestPath
							if (!((wp.getX() == bestPath.get(bestPath.size()-1).getX()) && (wp.getY() == bestPath.get(bestPath.size()-1).getY())))
							{
								bestPath.add(wp);
							}
							
							//Si l'intersection est un point du chemin 2
							if (intersection.get(0) == pointPath2.getX() && intersection.get(1) == pointPath2.getY())
							{
								// on continue sur le chemin 2, à l'indice suivant l'intersection
								addWayPoints(destinationPath, bestPath, j+1);
							}
							// on continue direct sur le chemin 2
							else
							{
								addWayPoints(destinationPath, bestPath, j);
							}
							
							// on retourne le bestPath
							return bestPath;
						}
					}
				}	
			}
			//On est sur le dernier point du chemin 1 (point central), donc on ajoute tout le chemin 2 (qui contient déjà le point central)
			else
			{
				addWayPoints(destinationPath, bestPath, 0);
				System.out.println("Enregistre point central");
				return bestPath;
			}
		}
		// ON EST JAMAIS CENSE ARRIVER JUSQU'ICI !!!!!!!!!!!!!!!!!!!!!!
		System.out.println("Pas cense arriver la");
		return bestPath;
	}

	public ArrayList<NERoom> getPathsList() {
		return this.listOfAllRooms;
		
	}

	public NERoom getCurrentDestination() {
		return this.currentDestination;
		
	}

	public ArrayList<NERoom> getAllDestinations() {
		return listOfAllRooms;
		
	}
	
	
/*	
	//retourne la liste des points pour aller � une destination pr�cise
	public ArrayList<WayPoint> getAPath(int idPath)
	{
		//stockera les points du meilleur chemin possible
		ArrayList<WayPoint> bestPath = new ArrayList<WayPoint>();
		
		//le chemin vers la destination voulue
		ArrayList<WayPoint> destinationRoom = new ArrayList<WayPoint>(listOfAllRooms.get(idPath));
		
		//le chemin de l'endroit actuel au point central
		ArrayList<WayPoint> actualRoom = new ArrayList<WayPoint>(listOfAllRooms.get(idActualRoom));
		
		//on lit la liste dans le sens inverse pour aller au point central
		for(int compteur = actualRoom.size()-1;compteur>=0;compteur--)
		{
			//on stock le point du chemin 1
			WayPoint actualPointPathOne = actualRoom.get(compteur);
			
			//On ajoute le point qu'on test
			bestPath.add(actualPointPathOne);
			
			//PREMIER CAS : point commun entre les deux chemins
			//on cherche l'indice du waypoint
			int indice = destinationRoom.wayPointIndice(actualRoom.get(compteur));
			
			//si le point appartient au deuxi�me chemin, on ajoute toute la fin du deuxi�me chemin
			if(indice != -1)
			{
				//On reprend le deuxi�me chemin
				addWayPoints(destinationRoom, bestPath, compteur);
				
				//on a trouv� le meilleur chemin que l'on retourne
				return bestPath;
			}
			//DEUXIEME CAS : pas de point commun
			else
			{
				//on stock le point suivant du chemin 1
				WayPoint nextPointPathOne = actualRoom.get(compteur+1);
				
				//si on est en deplacement verticale dans le chemin 1 (x1 = x2)
				if(actualPointPathOne.getX()==nextPointPathOne.getX())
				{
					//On cherche dans tout le chemin deux si on a un point avec le meme X
					for(int i = 0; i<destinationRoom.size();i++)
					{
						//si on a le meme X
						if(destinationRoom.get(i).getX()==actualPointPathOne.getX())
						{
							//si on a le point du chemin 2 qui est compris entre le point actuel et le suivant du chemin 1
							if(between(destinationRoom.get(i).getY(),actualPointPathOne.getY(),nextPointPathOne.getY()))
							{
								//On reprend le deuxi�me chemin
								addWayPoints(destinationRoom, bestPath, i);
								
								//on a trouv� le meilleur chemin que l'on retourne
								return bestPath;
							}
							//sinon on regarde si le point actuel est compris entre le point du chemin 2 et son suivant 
							else if(between(actualPointPathOne.getY(),destinationRoom.get(i).getY(),destinationRoom.get(i+1).getY()))
							{
								//on reprend le deuxi�me chemin
								addWayPoints(destinationRoom, bestPath, i);
								
								//on a trouv� le meilleur chemin que l'on retourne
								return bestPath;
							}
						}
						//else if X1=X2
						// et X1 entre x1 et x2
						// et y1 entre Y1 et Y2
						// alors on ajoute WayPoint(X1, y1) et on passe sur chemin 2
					}
				}
				// else cas d�placement horizontal (idem en inversant x et y)
			}
		}
		
		return bestPath;
	}
*/

}